#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <arpa/inet.h>
#include <semaphore.h>
#include <math.h>
#include "bzrc.h"

#define BUFLEN 128

// Globals
int debug = 0;
sem_t socketMutex;

// Support Routines
int sendQueryCommand(FILE* fSocket, char* command, char* response);
int sendActionCommand(FILE* fSocket, char* command);
int getItemCount(char* response);
int splitObstacle(obstacle** obstacles, int obstacleCount, int index);

// Connection
int BZRCConnect(char* address, int port, int* retSocket, FILE** retFSocket) {
	int tcpSocket = 0;
	FILE* fSocket = 0;
	struct sockaddr_in serverAddress;
	char buffer[BUFLEN];
	memset(buffer, 0, sizeof(buffer));
	memset((char *)&serverAddress, 0, sizeof(serverAddress));

	tcpSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (tcpSocket < 0) {
		fprintf(stderr, "Failed to create TCP socket\n");
		exit(-1);
	}

	if (!inet_aton(address, &serverAddress.sin_addr)) {
		fprintf(stderr, "Failed to convert address\n");
		exit(-1);
	}
	serverAddress.sin_family = AF_INET;
	serverAddress.sin_port = htons(port);

	if (connect(tcpSocket, (const struct sockaddr *)&serverAddress, sizeof(serverAddress)) < 0) {
		fprintf(stderr, "Could not connect to server\n");
		exit(-1);
	}

	fSocket = fdopen(tcpSocket, "r+");
	if (fgets(buffer, BUFLEN, fSocket) == NULL) {
		fprintf(stderr, "No handshake from server\n");
		exit(-1);
	}
	sprintf(buffer, "agent 1\n");
	fputs(buffer,fSocket);
	if (debug) printf("Connection established\n");
	*retSocket = tcpSocket;
	*retFSocket = fSocket;
	sem_init(&socketMutex,0, 1);
	return 0;
}

int BZRCDisconnect(int tcpSocket, FILE* fSocket) {
	fclose(fSocket);
	shutdown(tcpSocket, SHUT_RDWR);
	sem_destroy(&socketMutex);
	if (debug) printf("Connection closed\n");
	return 0;
}

// Support Routines
int getItemCount(char* response) {
	int itemCount = 0;
	char* tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		itemCount++;
		// Hack to repair string after strtok() call
		if (tokenPtr != response) *(tokenPtr-1) = '\n';
		tokenPtr = strtok(NULL,"\n");
	}
	if (debug) printf("%d items found\n", itemCount);
	return itemCount;
}

int sendActionCommand(FILE* fSocket, char* command) {
	char buffer[BUFLEN];
	int success = 0;
	// Send Command
	sem_wait(&socketMutex);
	if (debug) printf("Sending command: %s", command);
	fputs(command, fSocket);

	// Get command acknowledgement
	if (fgets(buffer, BUFLEN, fSocket) == NULL) {
		fprintf(stderr, "No acknowlegement for command: %s\n", command);
		exit(-1);
	}
	if (debug) printf("Server sent acknowledgement: %s", buffer);

	// Get command response
	if (fgets(buffer, BUFLEN, fSocket) == NULL) {
		fprintf(stderr, "No response for command: %s\n", command);
		exit(-1);
	}
	sem_post(&socketMutex);

	if (debug) printf("Server sent response: %s", buffer);

	// Make sure command was successful
	if (strncmp("ok", buffer, 2) != 0) {
		fprintf(stderr, "Warning - Fail response recieved: %s", buffer);
		success = -1;
	}
	return success;
}

int sendQueryCommand(FILE* fSocket, char* command, char* response) {
	char buffer[BUFLEN];
	int line = 0;
	int success = 0;
	response[0] = '\0'; // null-terminate response
	// Send Command
	sem_wait(&socketMutex);
	if (debug) printf("Sending command: %s", command);
	fputs(command, fSocket);

	// Get command acknowledgement
	if (fgets(buffer, BUFLEN, fSocket) == NULL) {
		fprintf(stderr, "No acknowlegement for command: %s\n", command);
		exit(-1);
	}
	if (debug) printf("Server sent acknowledgement: %s", buffer);

	// Get command response
	while (fgets(buffer, BUFLEN, fSocket) != NULL) {
		if (debug) printf("Server response line %d: %s", line, buffer);
		if (strncmp("fail", buffer, 4) == 0) {
			// failed command
			fprintf(stderr, "Warning - Fail response recieved: %s", buffer);
			success = -1;
			break;
		}
		else if (strncmp("begin", buffer, 5) == 0) {
			// started batch response
		}
		else if (strncmp("end", buffer, 3) == 0) {
			// ended batch response
			break;
		}
		else {
			strcat(response, buffer);
		}
		line++;
	}
	sem_post(&socketMutex);
	if (success == 0 && !line) {
		fprintf(stderr, "No response for command: %s\n", command);
		exit(-1);
	}
	return success;
}

// Actions
int shoot(FILE* fSocket, int entID) {
	char buffer[BUFLEN];
	sprintf(buffer, "shoot %d\n", entID);
	sendActionCommand(fSocket, buffer);
	return 0;
}

int setSpeed(FILE* fSocket, int entID, float value) {
	char buffer[BUFLEN];
	sprintf(buffer, "speed %d %f\n", entID, value);
	sendActionCommand(fSocket, buffer);
	return 0;
}

int setAngVel(FILE* fSocket, int entID, float value) {
	char buffer[BUFLEN];
	sprintf(buffer, "angvel %d %f\n", entID, value);
	sendActionCommand(fSocket, buffer);
	return 0;
}

// UNSUPPORTED IN THEIR VERSION OF THE SERVER
int setAccel(FILE* fSocket, int entID, int axis, float value) {
	char buffer[BUFLEN];
	char axisChar = (axis == 1) ? 'x' : 'y';
	sprintf(buffer, "accel%c %d %f\n", axisChar,entID, value);
	sendActionCommand(fSocket, buffer);
	return 0;
}

// Queries
int getTeams(FILE* fSocket, team** retTeams, int* retTeamCount) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(256);
	char* tokenPtr;
	int itemCount = 0;
	int currentIdx = 0;
	team* teams;
	sprintf(buffer, "teams\n");
	sendQueryCommand(fSocket, buffer, response);

	// Parse response
	itemCount = getItemCount(response);
	teams = (team*)malloc(sizeof(team) * itemCount);
	tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		sscanf(tokenPtr, "%s %s %d\n", buffer, teams[currentIdx].color, &teams[currentIdx].playerCount);
		tokenPtr = strtok(NULL, "\n");
		currentIdx++;
	}

	free(response);
	*retTeams = teams;
	*retTeamCount = itemCount;
	return 0;
}

/*int breakObstacle(obstacle** obstacles, int obstacleCount, int index) {
	int width = (*obstacles)[index].corners[1].x - (*obstacles)[index].corners[2].x;
	int height = (*obstacles)[index].corners[0].y - (*obstacles)[index].corners[1].y;
	int divisions;
	int i;
	obstacle* newObstacles;
	if (height > width) {
		divisions = height / width;
	}
	*obstacles = (obstacle*)realloc(obstacles, sizeof(obstacle)*(obstacleCount+divisions));
	newObstacles = *obstacles;
	pair oldBottom;
	oldBottom.x = newObstacles[index].corners[1].x
	oldBottom.y = newObstacles[index].corners[1].y
	for (i = 0; i < divisions; i++) {
		if (i == 0) {
			newObstacles[index].corners[1].y -= width;
			newObstacles[index].corners[2].y -= width;
		}
		else if (i == divisions - 1) {
			newObstacles[index].corners[1].x = oldBottom.x;
			newObstacles[index].corners[2].y = oldBottom.y;
		}
		else {
			// Forget it.
		}
		index++;
	}
	return index;
}*/

int splitObstacle(obstacle** obstacles, int obstacleCount, int index) {
	float width = (*obstacles)[index].corners[1].x - (*obstacles)[index].corners[2].x;
	float height = (*obstacles)[index].corners[0].y - (*obstacles)[index].corners[1].y;
	obstacle* newObstacles;
	*obstacles = (obstacle*)realloc(*obstacles, sizeof(obstacle)*(obstacleCount+1));
	newObstacles = *obstacles;
	float top = newObstacles[index].corners[0].y;
	float bottom = newObstacles[index].corners[1].y;
	float midpoint = top - (top - bottom) / 2;
	pair center;
	float radius;
	newObstacles[index].corners[1].y = midpoint;
	newObstacles[index].corners[2].y = midpoint;

	width = newObstacles[index].corners[1].x - newObstacles[index].corners[2].x;
	height = newObstacles[index].corners[0].y - newObstacles[index].corners[1].y;
	center.x = width / 2 + newObstacles[index].corners[2].x;
	center.y = height / 2 + newObstacles[index].corners[2].y;
	radius = sqrt(pow(width / 2,2) + pow(height / 2,2));
	newObstacles[index].radius = radius;
	newObstacles[index].center = center;

	newObstacles[index+1].corners[0].x = newObstacles[index].corners[0].x;
	newObstacles[index+1].corners[0].y = midpoint;
	newObstacles[index+1].corners[3].x = newObstacles[index].corners[3].x;
	newObstacles[index+1].corners[3].y = midpoint;
	newObstacles[index+1].corners[1].x = newObstacles[index].corners[1].x;
	newObstacles[index+1].corners[1].y = bottom;
	newObstacles[index+1].corners[2].x = newObstacles[index].corners[2].x;
	newObstacles[index+1].corners[2].y = bottom;
	return index+1;
}

int getObstacles(FILE* fSocket, obstacle** retObstacles, int* retObstacleCount) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(8*1024);
	char* tokenPtr;
	int itemCount = 0;
	int currentIdx = 0;
	obstacle* obstacles;
	float radius;
	pair center;
	float width;
	float height;
	sprintf(buffer, "obstacles\n");
	sendQueryCommand(fSocket, buffer, response);

	// Parse response
	itemCount = getItemCount(response);
	obstacles = (obstacle*)malloc(sizeof(obstacle) * itemCount);
	tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		sscanf(tokenPtr, "%s %f %f %f %f %f %f %f %f\n", 
			buffer, 
			&obstacles[currentIdx].corners[0].x,
			&obstacles[currentIdx].corners[0].y,
			&obstacles[currentIdx].corners[1].x,
			&obstacles[currentIdx].corners[1].y,
			&obstacles[currentIdx].corners[2].x,
			&obstacles[currentIdx].corners[2].y,
			&obstacles[currentIdx].corners[3].x,
			&obstacles[currentIdx].corners[3].y
		);
		width = obstacles[currentIdx].corners[1].x - obstacles[currentIdx].corners[2].x;
		height = obstacles[currentIdx].corners[0].y - obstacles[currentIdx].corners[1].y;
		center.x = width / 2 + obstacles[currentIdx].corners[2].x;
		center.y = height / 2 + obstacles[currentIdx].corners[2].y;
		radius = sqrt(pow(width / 2,2) + pow(height / 2,2));
		obstacles[currentIdx].radius = radius;
		obstacles[currentIdx].center = center;
		if (height >= (2* width)) {
			currentIdx = splitObstacle(&obstacles, itemCount, currentIdx);
			center.x = width / 2 + obstacles[currentIdx].corners[2].x;
			center.y = height / 2 + obstacles[currentIdx].corners[2].y;
			radius = sqrt(pow(width / 2,2) + pow(height / 2,2));
			obstacles[currentIdx].radius = radius;
			obstacles[currentIdx].center = center;
			itemCount++;
		}
		
		tokenPtr = strtok(NULL, "\n");
		currentIdx++;
	}

	free(response);
	*retObstacles = obstacles;
	*retObstacleCount = itemCount;
	return 0;
}

int getBases(FILE* fSocket, base** retBases, int* retBaseCount) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(512);
	char* tokenPtr;
	int itemCount = 0;
	int currentIdx = 0;
	base* bases;
	sprintf(buffer, "bases\n");
	sendQueryCommand(fSocket, buffer, response);

	// Parse response
	itemCount = getItemCount(response);
	bases = (base*)malloc(sizeof(base) * itemCount);
	tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		sscanf(tokenPtr, "%s %s %f %f %f %f %f %f %f %f\n", 
			buffer, 
			bases[currentIdx].color,	
			&bases[currentIdx].corners[0].x,
			&bases[currentIdx].corners[0].y,
			&bases[currentIdx].corners[1].x,
			&bases[currentIdx].corners[1].y,
			&bases[currentIdx].corners[2].x,
			&bases[currentIdx].corners[2].y,
			&bases[currentIdx].corners[3].x,
			&bases[currentIdx].corners[3].y
		);
		tokenPtr = strtok(NULL, "\n");
		currentIdx++;
	}

	free(response);
	*retBases = bases;
	*retBaseCount = itemCount;
	return 0;
}

int getFlags(FILE* fSocket, flag** retFlags, int* retFlagCount) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(512);
	char* tokenPtr;
	int itemCount = 0;
	int currentIdx = 0;
	flag* flags;
	sprintf(buffer, "flags\n");
	sendQueryCommand(fSocket, buffer, response);

	// Parse response
	itemCount = getItemCount(response);
	flags = (flag*)malloc(sizeof(flag) * itemCount);
	tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		sscanf(tokenPtr, "%s %s %s %f %f\n", 
			buffer, 
			flags[currentIdx].ownerColor,
			flags[currentIdx].holderColor,
			&flags[currentIdx].location.x,
			&flags[currentIdx].location.y
		);
		tokenPtr = strtok(NULL, "\n");
		currentIdx++;
	}

	free(response);
	*retFlags = flags;
	*retFlagCount = itemCount;
	return 0;
}

int getShots(FILE* fSocket, shot** retShots, int* retShotCount) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(512);
	char* tokenPtr;
	int itemCount = 0;
	int currentIdx = 0;
	shot* shots;
	sprintf(buffer, "shots\n");
	sendQueryCommand(fSocket, buffer, response);

	// Parse response
	itemCount = getItemCount(response);
	shots = (shot*)malloc(sizeof(shot) * itemCount);
	tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		sscanf(tokenPtr, "%s %f %f %f %f\n", 
			buffer, 
			&shots[currentIdx].location.x,
			&shots[currentIdx].location.y,
			&shots[currentIdx].velocity.x,
			&shots[currentIdx].velocity.y
		);
		tokenPtr = strtok(NULL, "\n");
		currentIdx++;
	}

	free(response);
	*retShots = shots;
	*retShotCount = itemCount;
	return 0;
}

int getMyTanks(FILE* fSocket, tank** retTanks, int* retTankCount) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(2048);
	char* tokenPtr;
	int itemCount = 0;
	int currentIdx = 0;
	tank* tanks;
	sprintf(buffer, "mytanks\n");
	sendQueryCommand(fSocket, buffer, response);

	// Parse response
	itemCount = getItemCount(response);
	tanks = (tank*)malloc(sizeof(tank) * itemCount);
	tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		sscanf(tokenPtr, "%s %d %s %s %d %f %s %f %f %f %f %f %f\n", 
			buffer, 
			&tanks[currentIdx].index,
			tanks[currentIdx].callsign,
			tanks[currentIdx].status,
			&tanks[currentIdx].shotsAvailble,
			&tanks[currentIdx].reloadTime,
			tanks[currentIdx].flag,
			&tanks[currentIdx].location.x,
			&tanks[currentIdx].location.y,
			&tanks[currentIdx].angle,
			&tanks[currentIdx].velocity.x,
			&tanks[currentIdx].velocity.y,
			&tanks[currentIdx].angVel
		);
		tokenPtr = strtok(NULL, "\n");
		currentIdx++;
	}

	free(response);
	*retTanks = tanks;
	*retTankCount = itemCount;
	return 0;
}

int getOtherTanks(FILE* fSocket, enemyTank** retTanks, int* retTankCount) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(4048);
	char* tokenPtr;
	int itemCount = 0;
	int currentIdx = 0;
	enemyTank* tanks;
	sprintf(buffer, "othertanks\n");
	sendQueryCommand(fSocket, buffer, response);

	// Parse response
	itemCount = getItemCount(response);
	tanks = (enemyTank*)malloc(sizeof(enemyTank) * itemCount);
	tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		sscanf(tokenPtr, "%s %s %s %s %s %f %f %f\n", 
			buffer, 
			tanks[currentIdx].callsign,
			tanks[currentIdx].color,
			tanks[currentIdx].status,
			tanks[currentIdx].flag,
			&tanks[currentIdx].location.x,
			&tanks[currentIdx].location.y,
			&tanks[currentIdx].angle
		);
		tokenPtr = strtok(NULL, "\n");
		currentIdx++;
	}

	free(response);
	*retTanks = tanks;
	*retTankCount = itemCount;
	return 0;
}

int getConstants(FILE* fSocket, constant** retConstants, int* retConstantCount) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(2048);
	char* tokenPtr;
	int itemCount = 0;
	int currentIdx = 0;
	constant* constants;
	sprintf(buffer, "constants\n");
	sendQueryCommand(fSocket, buffer, response);

	// Parse response
	itemCount = getItemCount(response);
	constants = (constant*)malloc(sizeof(constant) * itemCount);
	tokenPtr = strtok(response,"\n");
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		sscanf(tokenPtr, "%s %s %s\n", 
			buffer, 
			constants[currentIdx].name,
			constants[currentIdx].value
		);
		tokenPtr = strtok(NULL, "\n");
		currentIdx++;
	}

	free(response);
	*retConstants = constants;
	*retConstantCount = itemCount;
	return 0;
}

int getOccGrid(FILE* fSocket, occgrid* grid, int entID) {
	char buffer[BUFLEN];
	char* response = (char*)malloc(2* 8096);
	char* gridData;
	char* tokenPtr;
	int totalGridSize;
	int posX;
	int posY;
	int sizeX;
	int sizeY;
	int i = 0;
	sprintf(buffer, "occgrid %d\n", entID);
	if (sendQueryCommand(fSocket, buffer, response)) {
		return -1; // Occgrid failed
	}

	// Parse response
	tokenPtr = strtok(response,"\n");

	sscanf(tokenPtr, "at %d,%d\n", &posX, &posY);
	if (debug) printf("%s\n", tokenPtr);
	tokenPtr = strtok(NULL, "\n");

	sscanf(tokenPtr, "size %dx%d\n", &sizeX, &sizeY);
	if (debug) printf("%s\n", tokenPtr);
	tokenPtr = strtok(NULL, "\n");

	totalGridSize = sizeX * sizeY;
	gridData = (char*)malloc(totalGridSize);
	while (tokenPtr != NULL) {
		if (debug) printf("%s\n", tokenPtr);
		memcpy(gridData + i, tokenPtr, sizeY);
		tokenPtr = strtok(NULL, "\n");
		i = i + sizeY;
	}

	// Set struct values
	grid->topleft.x = posX;
	grid->topleft.y = posY;
	grid->size.x = sizeX;
	grid->size.y = sizeY;
	grid->grid = gridData;

	free(response);
	return 0;
}

